/***************************************************************************
 创建者: 华磊
 开始时间: 2019.3.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2017.5.8 类的具体实现代码编写
 ***************************************************************************/
#include "fleetservice.h"
#include "../BatteryMonitor/batterymonitor.h"
#include "mccontrollerthread.h"
#include "powermanager.h"
#include "messagelog.h"
#include "GeneralDefine.h"
#ifdef D_USE_ROS
#include "pathplanmanager.h"
#endif
#include "fleettextcommandtransfer.h"
#define D_CHARGE_DO_INDEX 3

extern BatteryMonitor *g_batteryMonitor;

FleetService::FleetService(PowerManager *powerManageIn,QObject *parent) : QObject(parent)
{
    errorCode=0;
    isInitialOk=false;
    powerManage=powerManageIn;
    robotId=-1;
    taskStatus=E_FLEET_TASK_STATUS_IDILE;
    isInCharging=false;
    isAgvLoaded=false;
    isLoopIdile=false;
    isUsePathAuthority=false;
    commandIndex=0;
    executingLine=0;
    executingCommandType=E_FLEET_COMMAND_NONE;
//    waitFlag=false;
    cu1PositionAllowAngleAccuracy=0.1;
    trafficCeaseFlag=false;
    scantrigPoseGetFlag=false;
//    passedStationId=-1;
//    nextStationId=-1;
//    currentStationId=-1;
//    passedPathId=-1;
//    nextPathId=-1;
//    currentPathId=-1;
    QString tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
    QString xmlfile = tmpFilePath+ROBOT_COMMON_NAME+QString::number( 1 )
                                                    +"/config/mcControllerInterpreter.xml";
    int tmpReturn=readConfigFile_mc(xmlfile);
    txtCommandTransfer=new  FleetTextCommandTransfer(this,10);
    if(1!=tmpReturn)
    {
        addMsg( ENUM_MSG_ERROR, "MoveFreeInstructionLogic", "MoveFreeInstructionLogic", 12201, robotId);
    }
    else
    {
        QString fleetConfigFile=D_TCRCONTROLLER_DATA_PATH;
        fleetConfigFile+=D_PATH_FLEETSERVICE_CONFIG_FILE_PATH;
        QString tmpStrComment;
        if(1!=initialConfigFile_fleet(fleetConfigFile,tmpStrComment))
        {
            addMsg( ENUM_MSG_ERROR, "MoveFreeInstructionLogic",
                    "MoveFreeInstructionLogic", 12215, robotId,0,0,0,0,tmpStrComment);
        }

        #ifdef D_USE_ROS
        QString planConfigFile=D_TCRCONTROLLER_DATA_PATH;
        planConfigFile+=D_PATH_PLANE_CONFIG_FILE_PATH;
        pathPlanManager=new PathPlanManager(planConfigFile);
        #endif

        isInitialOk=true;
        createThread();
        if(isUsePathAuthority)
        {
            createThread_traffic();
        }

    }


}

int FleetService::addMonitorDi_fleet(bool isOr, QVector<int> diIndex, QVector<int> diValue)
{
    return powerManage->addMonitorDi_fleet(userInfo,isOr,diIndex,diValue);
}

int FleetService::getTrigPosition(QQueue<PointPro> &trigPoseOut)
{
    return powerManage->getTrigPosition(trigPoseOut);
}

int FleetService::resetScanTrigPoseGetFlag()
{
    scantrigPoseGetFlag=false;
    return 1;
}

bool FleetService::getScantrigPoseGetFlag()
{
    return scantrigPoseGetFlag;
}

int FleetService::stopMotionTrig(int doIndex)
{

    int key=powerManage->stopMotionTrig(doIndex-1);
    scantrigPoseGetFlag=true;
    return key;
}

int FleetService::setMotionTrigPara(int doIndex, double trigPeriod)
{
    return powerManage->setMotionTrigPara(doIndex-1,trigPeriod);
}

int FleetService::setPathAuthority(QVector<int> pathListIn,QVector<int> stationAuthorityListIn)
{
    pathAuthorityMutex.lock();
    pathAuthorityList=pathListIn;
    stationAuthorityList=stationAuthorityListIn;
    pathAuthorityMutex.unlock();
    return 1;
}

int FleetService::getBarrierRunMode(E_BARRIER_RUN_MODE &modeOut)
{
    return powerManage->getBarrierRunMode(modeOut);
}

int FleetService::getBarriorDetectMode(E_BARRIER_TETECT_MODE &modeOut)
{
    return powerManage->getBarriorDetectMode(modeOut);
}

int FleetService::setCurrentVehicleBorderId(UserInfo userInfo, int idIn)
{
    int returnFlag;
    return powerManage->setCurrentVehicleBorderId(userInfo,idIn,returnFlag);
}

int FleetService::getCurrentVehicleBorderId(UserInfo userInfo, int &idOut)
{
    return powerManage->getCurrentVehicleBorderId(userInfo,idOut);
}

int FleetService::shutDownAgvSystem(UserInfo userInfo)
{
    if(1!=checkAuthority(userInfo))
    {
        qDebug()<<"checkAuthority failed,shutDownAgvSystem";
        return -1;
    }
    E_AGV_STATE stateOut;
    getCurrentAgvState(userInfo,stateOut);
    if(E_AGV_STATE_IDILE==stateOut)
    {
        powerManage->controllerShutDown();
    }
    else
    {
        addMsg( ENUM_MSG_WARNING, "FleetService", "FleetService", 12208, robotId);
        return 0;
    }


    return 1;
}

EN_CONTROL_TYPE FleetService::getRunModel()
{
    return powerManage->getRunModel();
}

int FleetService::getRollerMechanismStatus(E_ROLLER_STATUS &statusOut)
{
    return powerManage->getRollerMechanismStatus(userInfo,statusOut);
}

int FleetService::getLiftMechanismStatus(E_LIFT_STATUS &statusOut)
{
    return powerManage->getLiftMechanismStatus(userInfo,statusOut);
}

int FleetService::setVehicleBorderOffset(UserInfo userInfo, double front_offsetIn, double back_offsetIn,
                                         double left_offsetIn, double right_offsetIn)
{
    if(1!=checkAuthority(userInfo))
    {
        qDebug()<<"checkAuthority failed,setVehicleBorderOffset";
        return -1;
    }

    powerManage->setVehicleBorderOffset(userInfo,front_offsetIn,back_offsetIn,left_offsetIn,right_offsetIn);
    return 1;
}

int FleetService::setBarrierRunMode(UserInfo userInfo, E_BARRIER_RUN_MODE modeIn)
{
    if(1!=checkAuthority(userInfo))
    {
        qDebug()<<"checkAuthority failed,2";
        return -1;
    }
    int returnFlag;
    powerManage->setBarrierRunMode(userInfo,modeIn,returnFlag);
    return returnFlag;
}

int FleetService::clearFleetCommandQueue()
{
    queueMutex.lock();
    fleetCommandQueue.clear();
    commandIndex=0;
    queueMutex.unlock();
    powerManage->setRunningNumber_virtualLineControl(0);
    return 1;
}

int FleetService::getFleetCommandQueue(QQueue<FleetCommand> &fleetCommandQueueOut)
{
    queueMutex.lock();
    fleetCommandQueueOut=fleetCommandQueue;
    queueMutex.unlock();
    return 1;
}

int FleetService::clearErrorAndRestoreRun(UserInfo userInfo)
{
    if(1!=checkAuthority(userInfo))
    {
        qDebug()<<"checkAuthority failed,22";
        return -1;
    }
    addClearMessage(userInfo);
    int tmpKey=addAutoDrive(userInfo,currentAutoDrive);
    if(1!=tmpKey)
    {
        return tmpKey;
    }
    sleep(1);
    run(userInfo);

    return tmpKey;
}

int FleetService::getCurrentFloorId()
{
#ifdef D_USE_ROS
return pathPlanManager->getCurrentFloorId();
#endif
}

int FleetService::getMapVersion()
{
    #ifdef D_USE_ROS
    return pathPlanManager->getMapVersion();
#endif
}

int FleetService::errorNotice()
{
    #ifdef D_USE_ROS
    return    pathPlanManager->errorOccured();
#endif
}

int FleetService::getCurrentFleetMap(string &mapInfoReturn)
{
#ifdef D_USE_ROS
return pathPlanManager->getCurrentFleetMap(mapInfoReturn);
#endif
}

int FleetService::getFleetMap(string mapNameIn, string &mapInfoReturn)
{
#ifdef D_USE_ROS
return pathPlanManager->getFleetMap(mapNameIn,mapInfoReturn);
#endif
}

int FleetService::writeFleetMap(string mapNameIn, string &mapInfoIn)
{
#ifdef D_USE_ROS
return pathPlanManager->writeFleetMap(mapNameIn,mapInfoIn);
#endif
}

int FleetService::setFleetTextCommandMaxLogLine(int maxIn)
{
    return txtCommandTransfer->setFleetTextCommandMaxLogLine(maxIn);
}

int FleetService::getCurrentMapName(std::string &mapNameOut)
{
    return powerManage->getCurrentMapName(userInfo,mapNameOut);
}

int FleetService::getPathMovingInfo(PathMovingInfo &infoOut)
{
    powerManage->getPathMovingInfo_virtualLineControl(pathMovingInfo);
    infoOut=pathMovingInfo;
    return 1;

}

int FleetService::getFleetCommandLog(std::queue<std::string> &fleetCommandLogOut)
{
    return txtCommandTransfer->getFleetCommandLog(fleetCommandLogOut);
}

int FleetService::executeTexCommand(UserInfo userInfo, QString txtCommandIn)
{
    return txtCommandTransfer->parserTexCommand(txtCommandIn);
}

int FleetService::executeTexCommandList(UserInfo userInfo, QStringList txtCommandIn,int errorLineOut)
{
    int tmpKey;
    for(int i=0;i<txtCommandIn.size();i++)
    {
        tmpKey=executeTexCommand(userInfo,txtCommandIn[i]);
        if(1!=tmpKey)
        {
           errorLineOut=i;
           addMsg( ENUM_MSG_ERROR, "FleetService", "FleetService",  12203, -1,0,0,0,0,txtCommandIn[i]);
           return tmpKey;
        }
    }
    return  1;
}

int FleetService::getMotionServerInfo(UserInfo userInfo, UpdateDataLv2 &updateDataLv2)
{
    return powerManage->getMotionServerInfo(userInfo,updateDataLv2);
}

int FleetService::getTargetPositionDistanceLeft(double &lineDistance, double &angleDistance)
{
    VirtualLineDebugInfo virtualInfoOut;
    powerManage->getVirtualLineControlDebugInfo(userInfo,virtualInfoOut);
    lineDistance=virtualInfoOut.absoluteDistance*1000.0;
    angleDistance=180.0*virtualInfoOut.absoluteAngle/M_PI;
    return 1;
}

int FleetService::getMessageInfoCode(int &errorCode, int &warnCode, int &noticeCode)
{
    return MessageLog::getInstance()->getMessageInfoCode(errorCode,warnCode, noticeCode);
}

int FleetService::getFleetDebugInfo(FleetDebugInfo &infoOut)
{
    infoOut.taskStatus=taskStatus;
    queueMutex.lock();
    infoOut.commandQueueSize=fleetCommandQueue.size();
    queueMutex.unlock();
    infoOut.isLoopIdile=isLoopIdile;
    infoOut.commandIndex=commandIndex;
    infoOut.executingLine=executingLine;
    infoOut.executingCommandType=executingCommandType;
    return 1;

}

int FleetService::pause(UserInfo userInfo)
{
    if(1!=checkAuthority(userInfo))
    {
        qDebug()<<"checkAuthority failed,23";
        return -1;
    }
    powerManage->motion_allCease();
//    powerManage->ceaseProgram();
    taskStatus=E_FLEET_TASK_STATUS_CEASE;
    return 1;
}

int FleetService::stop(UserInfo userInfo)
{
    if(1!=checkAuthority(userInfo))
    {
        qDebug()<<"checkAuthority failed,24";
        return -1;
    }
    stop();
    return 1;
}

int FleetService::stop()
{
    powerManage->motion_allStop();
    taskStatus=E_FLEET_TASK_STATUS_IDILE;
    errorCode=0;
    queueMutex.lock();
    fleetCommandQueue.clear();
    commandIndex=0;
    queueMutex.unlock();
    powerManage->setRunningNumber_virtualLineControl(0);
    int returnFlag;
    powerManage->programReset(userInfo,returnFlag);
    isLoopIdile=true;
    #ifdef D_USE_ROS
    pathPlanManager->errorOccured();
    #endif
    trafficCeaseFlag=false;
    return 1;
}

int FleetService::run(UserInfo userInfo)
{
    if(1!=checkAuthority(userInfo))
    {
        qDebug()<<"checkAuthority failed,25";
        return -1;
    }
    if(E_FLEET_TASK_STATUS_MOVING==taskStatus)
    {
        qDebug()<<"warnning:FleetService::run E_FLEET_TASK_STATUS_MOVING==taskStatus";
        return 1;
    }
    powerManage->motion_allStartRun();
    powerManage->setEngineStatus_fleet(ENUM_COMMAND_RUN);
    taskStatus=E_FLEET_TASK_STATUS_MOVING;
    trafficCeaseFlag=false;
    return 1;
}

int FleetService::startInternalProgram(UserInfo userInfo)
{
    if(1!=checkAuthority(userInfo))
    {
        qDebug()<<"checkAuthority failed,26";
        return -1;
    }
    if(0==powerManage->programRun_fleet())
    {
        return 1;
    }
    return -1;
}

int FleetService::ceaseInternalProgram(UserInfo userInfo)
{
    if(1!=checkAuthority(userInfo))
    {
        qDebug()<<"checkAuthority failed,27";
        return -1;
    }
    return powerManage->programCease_fleet();
}

int FleetService::stopInternalProgram(UserInfo userInfo)
{
    if(1!=checkAuthority(userInfo))
    {
        qDebug()<<"checkAuthority failed,28";
        return -1;
    }
    return powerManage->programReset_fleet();
}

int FleetService::setRegisterValue(UserInfo userInfo, int typeIn, int index, int subIndex, double valueIn)
{
    if(1!=checkAuthority(userInfo))
    {
        qDebug()<<"checkAuthority failed,29";
        return -1;
    }
    return powerManage->setRegisterValue(userInfo,typeIn,index,subIndex,valueIn);
}

int FleetService::setVisionTrigObjectLength(UserInfo userInfo, int visionIndex, double lengthIn)
{
    if(1!=checkAuthority(userInfo))
    {
        qDebug()<<"checkAuthority failed,30";
        return -1;
    }
    return powerManage->setVisionTrigObjectLength(userInfo,visionIndex,lengthIn);
}

int FleetService::loadInternalProgram(UserInfo userInfo,std::string programName)
{
    if(1!=checkAuthority(userInfo))
    {
        qDebug()<<"checkAuthority failed,3";
        return -1;
    }
    if(0==powerManage->setProgramFileName_fleet(programName))
    {
        return 1;
    }
    return -2;
}

int FleetService::getCurrentAgvPosition(UserInfo userInfo, VehiclePosition &positionOut)
{
    PositionInfo_hmi tmpPosition;
    powerManage->getVehiclePosition_specified(userInfo,E_VEHICLE_POSITION_TYPE_ROS,tmpPosition);
    positionOut=tmpPosition.position;
    return 1;
}

int FleetService::getCurrentAgvState(UserInfo userInfo,E_AGV_STATE &stateOut)
{
    //判断错误消息
    if(true==MessageLog::getInstance()->isErrorOccured())
    {
       stateOut= E_AGV_STATE_EMERGENCE_STOP;
       return 1;
    }

    //判断空闲？需要集合算法模块
    if(E_FLEET_TASK_STATUS_IDILE==taskStatus)
    {
        queueMutex.lock();
        if(0!=fleetCommandQueue.size())
        {
            stateOut= E_AGV_STATE_COMMAND_READY;
        }
        else
        {
            stateOut= E_AGV_STATE_IDILE;
        }
        queueMutex.unlock();
        return 1;
    }
    else if(E_FLEET_TASK_STATUS_CEASE==taskStatus)
    {
        stateOut= E_AGV_STATE_CEASE;
        return 1;
    }
    //超声波信息提取
    std::vector<double> safeStatus;
    bool isCheckEnable = false;
    bool isUltrasonicStop=false;
    bool isUltrasonicCease=false;
    if(powerManage->getUltrasonicSensorSafeStatus(safeStatus,isCheckEnable) == 1)
    {
        for(int i=0;i<safeStatus.size();i++)
        {
            if(0.001>safeStatus[i])
            {
                isUltrasonicStop=true;
            }
            else if(1>safeStatus[i])
            {
                isUltrasonicCease=true;
            }
        }
    }
    //激光信息提取
    std::vector<std::string > sensorName;
    std::vector<double> sensorSafeRatio;
    bool isLaserStop=false;
    bool isLaserCease=false;
    if(powerManage->getPointCloudSafeStatus(sensorName,sensorSafeRatio,isCheckEnable) == 1)
    {
        for(int i=0;i<sensorName.size();i++)
        {
            if(0.001>sensorSafeRatio[i])
            {
                isLaserStop=true;
            }
            else if(1>sensorSafeRatio[i])
            {
                isLaserCease=true;
            }
        }
    }
    //已经到达站点，则不判断壁障
    if(E_FLEET_TASK_STATUS_MOVING==taskStatus)
    {
        if(true==isLoopIdile)
        {
            //充电判断
            if(true==powerManage->getCustomDoStatus(D_CHARGE_DO_INDEX))
            {
                stateOut= E_AGV_STATE_CHARGING;
                return 1;
            }
            stateOut= E_AGV_STATE_IDILE;
            return 1;
        }
        //判断激光停止
        if(E_FLEET_TASK_STATUS_MOVING==taskStatus && true==isLaserStop)
        {
            stateOut= E_AGV_STATE_LASER_STOP;
            return 1;
        }
        //判断超声波停止
        else if(E_FLEET_TASK_STATUS_MOVING==taskStatus && true==isUltrasonicStop)
        {
            stateOut= E_AGV_STATE_ULTRASONIC_STOP;
            return 1;
        }
        //判断激光减速
        else if(E_FLEET_TASK_STATUS_MOVING==taskStatus && true==isLaserCease)
        {
            stateOut= E_AGV_STATE_LASER_CEASE;
            return 1;
        }
        //判断超声波减速
        else if(E_FLEET_TASK_STATUS_MOVING==taskStatus && true==isUltrasonicCease)
        {
            stateOut= E_AGV_STATE_ULTRASONIC_CEASE;
            return 1;
        }
        //正常运行
        else
        {
            stateOut= E_AGV_STATE_MOVING;
            return 1;
        }

    }

    return 1;

}

int FleetService::getLoadedState(UserInfo userInfo, bool &stateOut)
{
    stateOut=isAgvLoaded;
    return 1;
}

int FleetService::getBatteryInfo(UserInfo userInfo, BatteryInformation &batteryInfoOut)
{
    return powerManage->getBatteryInfo(batteryInfoOut);
}

int FleetService::getCurrentAgvSpeed(UserInfo userInfo, double &lineSpeed, double &rotateSpeed)
{
    VehicleVelocity velocityOut;
    powerManage->getVehicleSpeedFeedback(userInfo,velocityOut);
    lineSpeed=velocityOut.x_move;
    rotateSpeed=velocityOut.z_rotate;
    return 1;
}

int FleetService::getAllMessage(UserInfo userInfo,QStringList &messageListOut)
{
    std::vector<struct Message> messageVector;
    powerManage->updateControllerMessage(userInfo,messageVector);
    messageListOut.clear();
    QString messageCode;
    QString timeStr;

    for(int i=0;i<messageVector.size();i++)
    {
        QString levelStr;
        switch(messageVector[i].MessageLevel)
        {
        case ENUM_MSG_REMIND:
        {
            levelStr="提示";
            break;
        }
        case ENUM_MSG_ERROR:
        {
            levelStr="错误";
            break;
        }
        case ENUM_MSG_WARNING:
        {
            levelStr="警告";
            break;
        }
        default:
        {
            levelStr="未知级别";
        }
        }
        timeStr=longTimeToTimeString((messageVector[i].time));
        messageCode=levelStr+":消息码";
        messageCode+=QString::number(messageVector[i].messageCode)+",";
        messageListOut.append(timeStr+messageCode+QString::fromStdString(messageVector[i].MessageInformation));
    }
    return 1;
}

int FleetService::clearCommand(UserInfo userInfo)
{
    stop();
//    queueMutex.lock();
//    fleetCommandQueue.clear();
//    queueMutex.unlock();
//    isLoopIdile=true;
//    pathPlanManager->errorOccured();
    return 1;
}

int FleetService::addRobotArmMove(UserInfo userInfo, RobotArmMoveParameter paraIn)
{
    if(1!=checkAuthority(userInfo))
    {
        qDebug()<<"checkAuthority failed,4";
        return -1;
    }
    isLoopIdile=false;
    queueMutex.lock();
    FleetCommand tmpCommand;
    commandIndex++;
    tmpCommand.commandIndex=commandIndex;

    tmpCommand.commandType=E_FLEET_COMMAND_ROBOT_ARM_MOVE;
    tmpCommand.robotArmMovePara=paraIn;
    fleetCommandQueue.enqueue(tmpCommand);
    queueMutex.unlock();
    return 1;
}

int FleetService::addLaserPath(UserInfo userInfo, LaserPathParameter paraIn)
{
//    if(1!=checkAuthority(userInfo)) 单机程序会用
//    {
//        return -1;
//    }
    isLoopIdile=false;
    queueMutex.lock();
    FleetCommand tmpCommand;
    commandIndex++;
    tmpCommand.commandIndex=commandIndex;

    tmpCommand.commandType=E_FLEET_COMMAND_LASER_PATH;
    tmpCommand.laserPathPara=paraIn;
    fleetCommandQueue.enqueue(tmpCommand);
    queueMutex.unlock();
    return 1;
}

int FleetService::addMaganeticPath(UserInfo userInfo, MaganeticPathParameter paraIn)
{
    if(1!=checkAuthority(userInfo))
    {
        qDebug()<<"checkAuthority failed,5";
        return -1;
    }
    isLoopIdile=false;
    queueMutex.lock();
    FleetCommand tmpCommand;
    commandIndex++;
    tmpCommand.commandIndex=commandIndex;

    tmpCommand.commandType=E_FLEET_COMMAND_MAGANETIC_PATH;
    tmpCommand.maganeticPathPara=paraIn;
    fleetCommandQueue.enqueue(tmpCommand);
    queueMutex.unlock();
    return 1;
}

int FleetService::addRoller(UserInfo userInfo, RollerParameter paraIn)
{
    if(1!=checkAuthority(userInfo))
    {
        qDebug()<<"checkAuthority failed,6";
        return -1;
    }
    isLoopIdile=false;
    queueMutex.lock();
    FleetCommand tmpCommand;
    commandIndex++;
    tmpCommand.commandIndex=commandIndex;
    tmpCommand.commandType=E_FLEET_COMMAND_ROLLER;
    tmpCommand.rollerPara=paraIn;
    fleetCommandQueue.enqueue(tmpCommand);
    queueMutex.unlock();
    return 1;
}

int FleetService::addLifter(UserInfo userInfo, LiftParameter paraIn)
{
    if(1!=checkAuthority(userInfo))
    {
        qDebug()<<"checkAuthority failed,7";
        return -1;
    }
    isLoopIdile=false;
    queueMutex.lock();
    FleetCommand tmpCommand;
    commandIndex++;
    tmpCommand.commandIndex=commandIndex;
    tmpCommand.commandType=E_FLEET_COMMAND_LIFT;
    tmpCommand.liftPara=paraIn;
    fleetCommandQueue.enqueue(tmpCommand);
    queueMutex.unlock();
    return 1;
}

int FleetService::addWait(UserInfo userInfo)
{
    if(1!=checkAuthority(userInfo))
    {
        qDebug()<<"checkAuthority failed,8";
        return -1;
    }
    isLoopIdile=false;
    queueMutex.lock();
    FleetCommand tmpCommand;
    commandIndex++;
    tmpCommand.commandIndex=commandIndex;
    tmpCommand.commandType=E_FLEET_COMMAND_WAIT;
    fleetCommandQueue.enqueue(tmpCommand);
    queueMutex.unlock();
    return 1;
}

int FleetService::addCharge(UserInfo userInfo, ChargeParameter paraIn)
{
    if(1!=checkAuthority(userInfo))
    {
        qDebug()<<"checkAuthority failed,9";
        return -1;
    }
    isLoopIdile=false;
    queueMutex.lock();
    FleetCommand tmpCommand;
    commandIndex++;
    tmpCommand.commandIndex=commandIndex;
    tmpCommand.commandType=E_FLEET_COMMAND_CHARGE;
    tmpCommand.chargePara=paraIn;
    fleetCommandQueue.enqueue(tmpCommand);
    queueMutex.unlock();
    return 1;
}

int FleetService::addMusic(UserInfo userInfo, MusicParameter paraIn)
{
    if(1!=checkAuthority(userInfo))
    {
        qDebug()<<"checkAuthority failed,10";
        return -1;
    }
    return processMusicInstruction(paraIn);//
//    isLoopIdile=false;
//    queueMutex.lock();
//    FleetCommand tmpCommand;
//    commandIndex++;
//    tmpCommand.commandIndex=commandIndex;
//    tmpCommand.commandType=E_FLEET_COMMAND_MUSIC;
//    tmpCommand.musicPara=paraIn;
//    fleetCommandQueue.enqueue(tmpCommand);
//    queueMutex.unlock();
    return 1;
}

int FleetService::addLoadMap(UserInfo userInfo, LoadMapParameter paraIn)
{
    if(1!=checkAuthority(userInfo))
    {
        qDebug()<<"checkAuthority failed,11";
        return -1;
    }
    isLoopIdile=false;
    queueMutex.lock();
    FleetCommand tmpCommand;
    commandIndex++;
    tmpCommand.commandIndex=commandIndex;
    tmpCommand.commandType=E_FLEET_COMMAND_LOAD_MAP;
    tmpCommand.mapPara=paraIn;
    fleetCommandQueue.enqueue(tmpCommand);
    queueMutex.unlock();
    return 1;
}

int FleetService::addSetGlobalVelocity(UserInfo userInfo, double velIn)
{
//    if(1!=checkAuthority(userInfo))
//    {
//        qDebug()<<"checkAuthority failed,12";
//        return -1;
//    }
    int returnFlag;
    powerManage->setAutoSpeedPercent(  userInfo,  velIn,  returnFlag );
    return 1;
}

int FleetService::addClearMessage(UserInfo userInfo)
{
    if(1!=checkAuthority(userInfo))
    {
        qDebug()<<"checkAuthority failed,13";
        return -1;
    }
    stop();
    int returnFlag;
    powerManage->setAllMsgClear(userInfo,  returnFlag );
    qDebug()<<"FleetService::addClearMessage";
    return powerManage->servoOnCurrentRobot();

}

int FleetService::addAutoDrive(UserInfo userInfo, AutoDriveParameter paraIn)
{
    E_AGV_STATE stateOut;
    getCurrentAgvState(userInfo,stateOut);
    if(E_AGV_STATE_IDILE!=stateOut)
    {
        addMsg( ENUM_MSG_WARNING, "FleetService", "FleetService",  12206, -1);
        qDebug()<<"warn,FleetService::addAutoDrive,,,E_FLEET_TASK_STATUS_IDILE!=taskStatus";
        return -1;
    }
    else
    {
        //判断命令队列是否有运动指令
        bool tmpHaveMoveCommand=false;
        queueMutex.lock();
        for(int i=0;i<fleetCommandQueue.size();i++)
        {
            if(E_FLEET_COMMAND_LASER_PATH==fleetCommandQueue[i].commandType
                    ||E_FLEET_COMMAND_MAGANETIC_PATH==fleetCommandQueue[i].commandType)
            {
                tmpHaveMoveCommand=true;
            }
        }
        queueMutex.unlock();
        if(tmpHaveMoveCommand)
        {
            addMsg( ENUM_MSG_WARNING, "FleetService", "FleetService",  12207, -1);
            qDebug()<<"warn,FleetService::addAutoDrive,,,tmpHaveMoveCommand true";
            return -1;
        }
    }
    currentAutoDrive=paraIn;
    #ifdef D_USE_ROS
//    addSetGlobalVelocity(userInfo,paraIn.speedRatio);

    PositionInfo_hmi tmpPosition_amcl,tmpPosition_vmark,tmpPosition_qrcode,tmpPosition_reflect;
    powerManage->getVehiclePosition_specified(userInfo,E_VEHICLE_POSITION_TYPE_ROS,tmpPosition_amcl);
    powerManage->getVehiclePosition_specified(userInfo,E_VEHICLE_POSITION_TYPE_VMARK,tmpPosition_vmark);
    powerManage->getVehiclePosition_specified(userInfo,E_VEHICLE_POSITION_TYPE_QRCODE,tmpPosition_qrcode);
    powerManage->getVehiclePosition_specified(userInfo,E_VEHICLE_POSITION_TYPE_CU1,tmpPosition_reflect);
    pathPlanManager->setPositionFeedback(tmpPosition_amcl,tmpPosition_vmark,tmpPosition_qrcode,tmpPosition_reflect);
    powerManage->getPathMovingInfo_virtualLineControl(pathMovingInfo);
    pathPlanManager->setStationPathStatus(pathMovingInfo);

    int errorCode,warnCode, noticeCode;
    MessageLog::getInstance()->getMessageInfoCode(errorCode,warnCode, noticeCode);
    if(0!=errorCode)
    {
        pathPlanManager->errorOccured();
    }

    QStringList pathCommandOut;
    int tmpNearstOut;
    int tmpStartId=pathMovingInfo.currentStationId;
    int methodOut;
    int tmpKey=pathPlanManager->getBestPath_byPathInfo(pathMovingInfo,paraIn.targetStationId,
                                                       pathCommandOut,tmpNearstOut,methodOut);
    switch(tmpKey)
    {
    case 1:
        if(0==methodOut)
        {
            addMsg( ENUM_MSG_REMIND, "FleetService", "FleetService",  12209, -1,
                                           tmpStartId,paraIn.targetStationId,pathMovingInfo.currentStationId,tmpNearstOut);
            qDebug()<<"FleetService::info,路径规划成功:sucess（正常开始站点规划）！！！"<<tmpStartId<<paraIn.targetStationId<<pathMovingInfo.currentStationId;
        }
        else if(1==methodOut)
        {
            addMsg( ENUM_MSG_REMIND, "FleetService", "FleetService",  12210, -1,
                                           tmpStartId,paraIn.targetStationId,pathMovingInfo.currentStationId,tmpNearstOut);
            qDebug()<<"FleetService::info,路径规划成功:sucess（找到最近点进行）！！！"<<tmpStartId<<paraIn.targetStationId<<pathMovingInfo.currentStationId;
        }
        else
        {
            addMsg( ENUM_MSG_REMIND, "FleetService", "FleetService",  12211, -1,
                                           tmpStartId,paraIn.targetStationId,pathMovingInfo.currentPathId);
            qDebug()<<"FleetService::info,路径规划成功:sucess（先运行完成当前路径）！！！"<<tmpStartId
                   <<paraIn.targetStationId<<pathMovingInfo.currentPathId;
        }
        int tmpLine;
        commandIndex=0;//将行号复位．
        powerManage->setRunningNumber_virtualLineControl(0);
        if(1==executeTexCommandList(userInfo,pathCommandOut,tmpLine))
        {
            return 1;
        }
        else
        {
            qDebug()<<"执行executeTexCommandList失败！！！";
            return -1;
        }
        break;
    case -105:
    {
        addMsg( ENUM_MSG_WARNING, "FleetService", "FleetService",  12214, -1,
                                       tmpStartId,paraIn.targetStationId,pathMovingInfo.currentPathId);
        return -15;
        break;
    }
    default:
        if(0==methodOut)
        {
            addMsg( ENUM_MSG_WARNING, "FleetService", "FleetService",  12204, -1,
                                           tmpStartId,paraIn.targetStationId,pathMovingInfo.currentStationId,tmpNearstOut);
            qDebug()<<"FleetService::error,正常开始站点规划无路径！！！"<<tmpStartId<<paraIn.targetStationId<<pathMovingInfo.currentStationId;
        }
        else if(1==methodOut)
        {
            addMsg( ENUM_MSG_REMIND, "FleetService", "FleetService",  12212, -1,
                                           tmpStartId,paraIn.targetStationId,pathMovingInfo.currentStationId,tmpNearstOut);
            qDebug()<<"FleetService::error,路径规划失败:（找到最近点进行）！！！"<<tmpStartId<<paraIn.targetStationId<<pathMovingInfo.currentStationId;
        }
        else
        {
            addMsg( ENUM_MSG_REMIND, "FleetService", "FleetService",  12213, -1,
                                           tmpStartId,paraIn.targetStationId,pathMovingInfo.currentPathId);
            qDebug()<<"FleetService::error,路径规划失败:（先运行完成当前路径）！！！"<<tmpStartId
                   <<paraIn.targetStationId<<pathMovingInfo.currentPathId;
        }

        return -2;
        break;
    }
    /*******
    int tmpStartId=pathMovingInfo.currentStationId;
    if(tmpStartId<0)
    {
        //样条曲线可能会有姿态问题。如果离开上一个站点后，姿态角度变化大，会有问题。可以多用几个点，来解决中途报错问题。
        tmpStartId=pathMovingInfo.passedStationId;
        //解决走了一半，中途返回问题．
        if(pathMovingInfo.passedStationId==paraIn.targetStationId)
        {
            tmpStartId=pathMovingInfo.nextStationId;
        }
    }
    int tmpNearstOut;
    if(1==pathPlanManager->getBestPath(tmpStartId,paraIn.targetStationId,pathCommandOut,tmpNearstOut))
    {
        int tmpLine;
        commandIndex=0;//将行号复位．
        powerManage->setRunningNumber_virtualLineControl(0);
        if(1==executeTexCommandList(userInfo,pathCommandOut,tmpLine))
        {
            return 1;
        }
        else
        {
            qDebug()<<"执行executeTexCommandList失败！！！";
            return -1;
        }
    }
    else
    {
        addMsg( ENUM_MSG_ERROR, "FleetService", "FleetService",  12204, -1,
                                       tmpStartId,paraIn.targetStationId,pathMovingInfo.currentStationId,tmpNearstOut);
        qDebug()<<"FleetService::error,无路径！！！"<<tmpStartId<<paraIn.targetStationId<<pathMovingInfo.currentStationId;
        return -2;
    }
    ****************************/
    return 1;
#endif

}

int FleetService::addWaitDi(UserInfo userInfo, WaitDiParameter paraIn)
{
    if(1!=checkAuthority(userInfo))
    {
        qDebug()<<"checkAuthority failed,20";
        return -1;
    }
    isLoopIdile=false;
    queueMutex.lock();
    FleetCommand tmpCommand;
    commandIndex++;
    tmpCommand.commandIndex=commandIndex;
    tmpCommand.commandType=E_FLEET_COMMAND_WAITDI;
    tmpCommand.waitDiPara=paraIn;
    fleetCommandQueue.enqueue(tmpCommand);
    queueMutex.unlock();
    return 1;
}

int FleetService::addSetDo(UserInfo userInfo, SetDoParameter paraIn)
{
    if(1!=checkAuthority(userInfo))
    {
        return -1;
    }
    isLoopIdile=false;
    queueMutex.lock();
    FleetCommand tmpCommand;
    commandIndex++;
    tmpCommand.commandIndex=commandIndex;
    tmpCommand.commandType=E_FLEET_COMMAND_SETDO;
    tmpCommand.setDoPara=paraIn;
    fleetCommandQueue.enqueue(tmpCommand);
    queueMutex.unlock();
    return 1;
}

int FleetService::loadMap(string mapPathIn)
{
 #ifdef D_USE_ROS
    return pathPlanManager->loadMap(mapPathIn);
#endif
}

int FleetService::setDoVector(unsigned short doListIn, unsigned short doValidMask)
{
    return powerManage->setDoVector(doListIn,doValidMask);
}

unsigned short FleetService::getDi_short(int shortIndex)
{
    return powerManage->getDi_short(shortIndex);
}

unsigned short FleetService::getDo_short(int shortIndex)
{
    return powerManage->getDo_short( shortIndex);
}


int FleetService::checkAuthority(UserInfo userInfo)
{
    if( 1!=powerManage->checkAuthority_run())
    {
        qDebug()<<"no authority";
        return -1;
    }

    if( ENUM_CONTROL_FLEET!=powerManage->getRunModel() && ENUM_CONTROL_EXT!=powerManage->getRunModel())
    {
        addMsg( ENUM_MSG_WARNING, "FleetService", "FleetService",  12202, -1);
        return -1;
    }
    return 1;
}

int FleetService::processWaitInstruction()
{
    powerManage->motion_allCease();
//    powerManage->ceaseProgram();
    taskStatus=E_FLEET_TASK_STATUS_CEASE;
    //如果不等待可能导致virtualLine被停止，从而站号不对。
    qDebug()<<"processWaitInstruction sleep 2";
    sleep(2);
    return 1;
}

int FleetService::processWaitDiInstruction(const WaitDiParameter &paraIn)
{
    bool tmpDiValue=powerManage->getCustomDiStatus(paraIn.diIndex-1);
    while(tmpDiValue!=paraIn.waitCondition)
    {
        usleep(10000);
        if(E_FLEET_TASK_STATUS_IDILE==taskStatus)
        {
            qDebug()<<"提前退出WaitDi";
            return 0;
        }
    }
    return 1;
}

int FleetService::processSetDoInstruction(const SetDoParameter &paraIn)
{
    int returnFlag;
    return powerManage->setDo(userInfo,paraIn.doIndex-1,paraIn.doValue,returnFlag);
}

int FleetService::processMusicInstruction(const MusicParameter &musicParaIn)
{
    return powerManage->playMusic(musicParaIn.musicFileName,musicParaIn.musicPlayTimes,musicParaIn.musicVolume);
}


int FleetService::processMapInstruction(const LoadMapParameter &mapParaIn)
{
    VehiclePosition position;
    position.x=mapParaIn.x;
    position.y=mapParaIn.y;
    position.rotate=mapParaIn.r*M_PI/180.0;
//    pathPlanManager->loadMap(mapParaIn.mapName.toStdString());
    return powerManage->loadMapAndSetOdometer(userInfo,mapParaIn.mapName,position);
}

int FleetService::processChargeInstruction(const ChargeParameter &chargeParaIn)
{
    return powerManage->executeChargeInstruction(userInfo,chargeParaIn);
}

int FleetService::processLiftInstruction(const LiftParameter &liftParaIn)
{
    return powerManage->executeLiftInstruction(userInfo,liftParaIn);
}

int FleetService::processRollerInstruction(const RollerParameter &rollerParaIn)
{
    return powerManage->executeRollerInstruction(userInfo,rollerParaIn);
}

int FleetService::processMaganeticInstruction(const MaganeticPathParameter &maganeticPathParaIn)
{
    return powerManage->executeMoveMaganeticInstruction(userInfo,maganeticPathParaIn);
}

//int FleetService::processNavLineContinusInstruction(const QVector<int> &lineList,
//                           const QVector<LaserPathParameter> &laserPathListParaIn, int isLastPointMove)
//{
//    //设置停车精度；距离0.02时，角度3度
//    powerManage->setAgvTargetAllowOffset(
//                laserPathListParaIn.last().allowAccuracy,2.6*laserPathListParaIn.last().allowAccuracy);
//    //设置轨迹策略
//    powerManage->setAgvPathFollowStrategy(laserPathListParaIn.last().pathControlStrategy);

//    //设置是否检测障碍物
//    if(false==laserPathListParaIn.last().isDetectBarrier)
//    {
//        powerManage->setUltrasonicSensorCheckEnable(false);
//        powerManage->setPointCloudCheckEnable(false);
//    }
//    else
//    {
//        powerManage->setUltrasonicSensorCheckEnable(true);
//        powerManage->setPointCloudCheckEnable(true);
//    }

//    powerManage->setMotionServerModel(ENUM_MOTION_MODEL_VEHICLE_VIRTUAL_LINE_TRACK);
//    //odom set
//    int tmpReturn;
//    if(0==laserPathListParaIn.last().moveFreeMode)
//    {
//        powerManage->setOdomSolver(E_ODOM_SOLVER_ENCODER,tmpReturn);
//    }
//    else if(2==laserPathListParaIn.last().moveFreeMode)
//    {
//        powerManage->setOdomSolver(E_ODOM_SOLVER_LASER,tmpReturn);
//    }


////    powerManage->startOneModel(ENUM_MOTION_MODEL_VEHICLE_VIRTUAL_LINE_TRACK);


//    //vmark设置
//    // 普通点
//    if(EM_MOVE_FREE_DP==laserPathListParaIn.last().pointSensorType)
//    {
//        if(1!=powerManage->changeToPositionType(E_VEHICLE_POSITION_TYPE_ROS))
//        {
//            quitClear();
//            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3904, robotId);
//            return -1;
//        }
//        powerManage->setCheckVehiclePosition(E_VEHICLE_POSITION_TYPE_ROS,true);
////        powerManage->setCheckVehiclePosition_vmark(false);
////        powerManage->setCheckVehiclePosition_amcl(true);
//        powerManage->setVehicleAcceleration(agv_lineMaxAcceleration);
//        powerManage->setVehicleAcceleration_rotate(agv_rotateMaxAcceleration);
//        powerManage->setVirtualLineEnableKpChange(true);

//    }
//    // VMark点
//    else if(EM_MOVE_FREE_VMARK == laserPathListParaIn.last().pointSensorType)
//    {
//        //激光扫描范围.切换可能有问题，ｖmark没有及时识别到。
//        if(1!=powerManage->setLaserIntensityFilterByType_visionMatch(laserPathListParaIn.last().laserTypeNum))
//        {
//            quitClear();
//            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3902, robotId);
//            return -1;
//        }

//        powerManage->setCurrentVMarkType(laserPathListParaIn.last().pointIndex);
//        if(1!=powerManage->changeToPositionType(E_VEHICLE_POSITION_TYPE_VMARK))
//        {
//            quitClear();
//            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3903, robotId );
//            return -1;
//        }
//        powerManage->setCheckVehiclePosition(E_VEHICLE_POSITION_TYPE_VMARK,true);
////        powerManage->setCheckVehiclePosition_vmark(true);
////        powerManage->setCheckVehiclePosition_amcl(false);
//        powerManage->setVehicleAcceleration(vmark_lineAcceleration);
//        powerManage->setVehicleAcceleration_rotate(vmark_rotateAcceleration);
//        powerManage->setVirtualLineEnableKpChange(false);

//    }
//    // 二维码点
//    else if(EM_MOVE_FREE_QR==laserPathListParaIn.last().pointSensorType)
//    {
//        if(1!=powerManage->changeToPositionType(E_VEHICLE_POSITION_TYPE_QRCODE))
//        {
//            quitClear();
//            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3909, robotId);
//            return -1;
//        }
//        powerManage->setCheckVehiclePosition(E_VEHICLE_POSITION_TYPE_QRCODE,true);
//        powerManage->setVehicleAcceleration(vmark_lineAcceleration);
//        powerManage->setVehicleAcceleration_rotate(vmark_rotateAcceleration);
//        powerManage->setVirtualLineEnableKpChange(false);
//    }
//    // CU1点
//    else if(EM_MOVE_FREE_CU1==laserPathListParaIn.last().pointSensorType)
//    {
//        if(1!=powerManage->changeToPositionType(E_VEHICLE_POSITION_TYPE_CU1))
//        {
//            quitClear();
//            addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3910, robotId);
//            return -1;
//        }
//        powerManage->setCheckVehiclePosition(E_VEHICLE_POSITION_TYPE_CU1,true);
//        powerManage->setVehicleAcceleration(vmark_lineAcceleration);
//        powerManage->setVehicleAcceleration_rotate(vmark_rotateAcceleration);
//        powerManage->setVirtualLineEnableKpChange(false);
//        //设置停车精度；
//        powerManage->setAgvTargetAllowOffset(
//                    laserPathListParaIn.last().allowAccuracy,cu1PositionAllowAngleAccuracy*M_PI/180.0);
//    }
//    else
//    {
//        quitClear();
//        addMsg( ENUM_MSG_ERROR, "ProgramEngine", "ProgramEngine",  3908, robotId );
//        return -1;
//    }

//    //添加轨迹指令

//    if(1!=addMotionInstruction_multiMove(lineList,laserPathListParaIn))
//    {
//        quitClear();
//        qDebug()<<"startMotion error,,,processMotionInstruction_multiMove";
//        return -1;
//    }


//    //完成判断，没有则阻塞等待

//    E_FLEET_TASK_STATUS taskStatus_old=E_FLEET_TASK_STATUS_MOVING;
//    for(int i=0;i<lineList.size();i++)
//    {
//        int isFinished=-1;
//        while(-1==isFinished)
//        {
//            //        waitFlag=true;
//                    if(1==isMotionInstructionFinished(lineList[i]))
//                    {
//                        isFinished=1;
//                    }

//                    if(-1==isFinished)
//                    {
//                        usleep(D_MOTION_INSTRUCTION_WAIT_SLEEP);
//                    }

//                    while(E_FLEET_TASK_STATUS_CEASE ==taskStatus)
//                    {
//                        if(E_FLEET_TASK_STATUS_MOVING==taskStatus_old && E_FLEET_TASK_STATUS_CEASE ==taskStatus)
//                        {
//                            taskStatus_old=E_FLEET_TASK_STATUS_CEASE;
//                        }
//                        usleep(D_MOTION_INSTRUCTION_WAIT_SLEEP);
//                    }
//                    if(E_FLEET_TASK_STATUS_CEASE==taskStatus_old && E_FLEET_TASK_STATUS_MOVING ==taskStatus)
//                    {
//                        taskStatus_old=E_FLEET_TASK_STATUS_MOVING;
//                    }

//                    if(E_FLEET_TASK_STATUS_IDILE ==taskStatus)
//                    {
//                        quitClear();
//                        return 0;
//                    }
//        }

//    }
////    waitFlag=false;
//    quitClear();
//    return  1;
//}


int FleetService::addMotionInstruction_multiMove(const QVector<int> &lineList,
                                    const QVector<LaserPathParameter> &laserPathListParaIn)
{
    qDebug()<<"FleetService::processMotionInstruction_multiMove";

    //加入指令，非平滑加一条，平滑指令加一完整段。
    QVector<NavParameter> tmpCommandVector;
    NavParameter tmpParameter;
    tmpParameter.velocityUnit=ENUM_RATIO;

    for(int j = 0; j < laserPathListParaIn.size();j++)
    {

            tmpParameter.moveType=E_NAV_COMMAND_LINE;
            tmpParameter.velocityUnit=ENUM_RATIO;
            tmpParameter.velocity=laserPathListParaIn[j].velocityRatio/100.0;
            tmpParameter.targetPoint.posX=laserPathListParaIn[j].targetPosition.x*1000.0;//单位ｍｍ
            tmpParameter.targetPoint.posY=laserPathListParaIn[j].targetPosition.y*1000.0;;//单位ｍｍ
            tmpParameter.targetPoint.posR=laserPathListParaIn[j].targetPosition.rotate*180.0/M_PI;//单位degree
            qDebug()<<"targetPoint x="<<tmpParameter.targetPoint.posX<<"mm y="<<tmpParameter.targetPoint.posY
                                 <<"mm r="<<tmpParameter.targetPoint.posR<<"degree";
            tmpParameter.cntRatio=laserPathListParaIn[j].CNT_Value;
            tmpParameter.lineNumber=lineList[j];
            tmpParameter.incFlag=0;
            tmpParameter.pathFlag=0;
            tmpParameter.isPositiveDirection=laserPathListParaIn[j].isPositiveDirection;
            if(D_MIN_NAV_CNT>tmpParameter.cntRatio && 0!=tmpParameter.cntRatio)
            {
//                qDebug()<<"programEngineSatus=="<<programEngine->getEngineStatue();
                addMsg( ENUM_MSG_ERROR, "ProgramEngine", "MoveFreeInstructionLogic",  3907, robotId
                        , D_MIN_NAV_CNT);
                qDebug()<<"fleet: D_MIN_NAV_CNT>tmpParameter.cntRatio && 0!=tmpParameter.cntRatio, cntRatio "
                       <<tmpParameter.cntRatio;
                return -1;
            }
            tmpCommandVector.append(tmpParameter);
            if(0==tmpParameter.cntRatio && j!=laserPathListParaIn.size()-1)
            {
                addMsg( ENUM_MSG_ERROR, "ProgramEngine", "MoveFreeInstructionLogic",  3907, robotId
                        , D_MIN_NAV_CNT);
                qDebug()<<"fleet: 0==tmpParameter.cntRatio && j!=laserPathListParaIn.size()-1,j "<<j;
                return -1;
//                programEngine->setMotionStage(E_PROGRAM_MOTION_PROCESS_INSTRUCTION_FINE_POINT_END);
                break;
            }

    }


    if( tmpCommandVector.size() )
    {
//        programEngine->setMotionInstructionAdded(tmpCommandVector.size());
        powerManage->addVirtualLineTrajectoryCommand(tmpCommandVector);
    }
    else
    {
//        programEngine->setMotionInstructionAdded(0);
    }

    return 1;
}


int FleetService::taskLoop()
{
    motionDebugInfo.commonThreadStage=E_COMMON_THREAD_TRAJECTORY_LOOP;
    checkEngineStatus();
    if(E_FLEET_TASK_STATUS_MOVING==taskStatus && 0==errorCode
            && ENUM_CONTROL_FLEET==powerManage->getRunModel())
    {
        int resultKey=processQueue();
        if(-1==resultKey)
        {
            stop();
            return -1;
        }
        else if(0==resultKey)
        {
            isLoopIdile=true;
            return 0;
        }
        isLoopIdile=false;
    }
    return 1;
}

int FleetService::trafficLoop()
{
    PathMovingInfo tmpPathInfo;
    powerManage->getPathMovingInfo_virtualLineControl(tmpPathInfo);

    pathAuthorityMutex.lock();
    QVector<int> tmpPathList=pathAuthorityList;
    QVector<int> tmpStationList=stationAuthorityList;
    pathAuthorityMutex.unlock();

    if(tmpPathInfo.currentStationId<0 && tmpPathInfo.currentPathId>0)
    {
        E_AGV_STATE stateOut;
        getCurrentAgvState(userInfo,stateOut);
        //当前路径或下一个站点未授权，则暂停
        if(false==tmpPathList.contains(tmpPathInfo.currentPathId) || false==tmpStationList.contains(tmpPathInfo.nextStationId))
        {
            if(false==trafficCeaseFlag && (E_AGV_STATE_IDILE!=stateOut && E_AGV_STATE_COMMAND_READY!=stateOut))
            {
                trafficCeaseFlag=true;
                pause(userInfo);
                if(false==tmpPathList.contains(tmpPathInfo.currentPathId))
                {
                    addMsg(ENUM_MSG_REMIND,"FleetService","FleetService",12216,robotId,tmpPathInfo.currentPathId);
                }
                else
                {
                    addMsg(ENUM_MSG_REMIND,"FleetService","FleetService",12218,robotId,tmpPathInfo.nextStationId);
                }

            }
        }
        else
        {
            if(trafficCeaseFlag)
            {
                run(userInfo);
                addMsg(ENUM_MSG_REMIND,"FleetService","FleetService",12217,robotId,tmpPathInfo.currentPathId);
            }
        }
    }

    return 1;

}

int FleetService::checkEngineStatus()
{

    if(ENUM_CONTROL_FLEET==powerManage->getRunModel())
    {
        if(ENUM_COMMAND_RESET==powerManage->getEngineStatue())
        {
            taskStatus=E_FLEET_TASK_STATUS_IDILE;
        }
        else if(ENUM_COMMAND_CEASE==powerManage->getEngineStatue())
        {
            taskStatus=E_FLEET_TASK_STATUS_CEASE;
        }
    }
    return 1;
}

int FleetService::getCommandFromQueue(FleetCommand &moveParameter)
{
    queueMutex.lock();//出队列前加锁
    if(fleetCommandQueue.isEmpty())
    {
        queueMutex.unlock();
        return 0;
    }
    moveParameter = fleetCommandQueue.dequeue();

    queueMutex.unlock();

    return 1;
}

int FleetService::getCommandQueueSize()
{
    int tmpSize;
    queueMutex.lock();;//出队列前加锁
    tmpSize=fleetCommandQueue.size();
    queueMutex.unlock();
    return tmpSize;
}

int FleetService::processQueue()
{
    motionDebugInfo.commonThreadStage=E_COMMON_THREAD_TRAJECTORY_CALCULATE_PATH;
    FleetCommand tmp_fleet_command;
    if(0==getCommandFromQueue(tmp_fleet_command))
    {
        //qDebug()<<"TrajectoryServer::calculateTrajectoryPath(), return 0";
        motionDebugInfo.commonThreadStage=E_COMMON_THREAD_TRAJECTORY_CALCULATE_PATH_NOCOMMAND;
        motionDebugInfo.commandQueueSize=0;
        return 0;
    }
    motionDebugInfo.commandQueueSize=getCommandQueueSize();

    executingLine=tmp_fleet_command.commandIndex;
    executingCommandType=tmp_fleet_command.commandType;
    int processResult=-1;
    int queueGetedFlag=0;
    QVector<LaserPathParameter> moveParameterArry;
    QVector<int> lineArry;
//    while(-1==processResult)
//    {
        motionDebugInfo.commonThreadStage=E_COMMON_THREAD_TRAJECTORY_CALCULATE_PATH_WHILE;
        switch(tmp_fleet_command.commandType)
        {
        case E_FLEET_COMMAND_ROBOT_ARM_MOVE:
        {
                QVector<RobotArmMoveParameter> armPointListParaArry;
                armPointListParaArry.append(tmp_fleet_command.robotArmMovePara);
                lineArry.append(tmp_fleet_command.commandIndex);
                processResult=powerManage->executeMoveRobotArm_run(userInfo,lineArry,armPointListParaArry);
                break;
        }
            case E_FLEET_COMMAND_LASER_PATH:
            {
                if(0==tmp_fleet_command.laserPathPara.CNT_Value)
                {//单条
                    moveParameterArry.append(tmp_fleet_command.laserPathPara);
                    lineArry.append(tmp_fleet_command.commandIndex);
//                    processResult=processNavLineContinusInstruction(lineArry,moveParameterArry,0);
                    processResult=powerManage->executeMultiPoint_run(userInfo,lineArry,moveParameterArry);
                    break;
                }
                else
                {//多条平滑

                    moveParameterArry.append(tmp_fleet_command.laserPathPara);
                    lineArry.append(tmp_fleet_command.commandIndex);
                    do
                    {
                        int tmpKey=getCommandFromQueue(tmp_fleet_command);
                        if(0!=tmpKey)
                        {
                           if(E_FLEET_COMMAND_LASER_PATH!=tmp_fleet_command.commandType)
                           {
                               addMsg(ENUM_MSG_ERROR,"FleetService","FleetService",12205,robotId);
                               return -1;
                           }
                           lineArry.append(tmp_fleet_command.commandIndex);
                           moveParameterArry.append(tmp_fleet_command.laserPathPara);
                           if(0==tmp_fleet_command.laserPathPara.CNT_Value)
                           {
//                              return processNavLineContinusInstruction(lineArry,moveParameterArry,0);
                              return powerManage->executeMultiPoint_run(userInfo,lineArry,moveParameterArry);
                           }

                        }
                        usleep(5000);
                        if(E_FLEET_TASK_STATUS_IDILE==taskStatus)
                        {
                            return 0;
                        }

                    }while(1);





                     break;
                }
            }


            case E_FLEET_COMMAND_MAGANETIC_PATH:
            {
                processResult=processMaganeticInstruction(tmp_fleet_command.maganeticPathPara);
                break;

            }
            case E_FLEET_COMMAND_MUSIC:
            {

                processResult=processMusicInstruction(tmp_fleet_command.musicPara);
                break;

            }
            case E_FLEET_COMMAND_ROLLER:
            {

                processResult=processRollerInstruction(tmp_fleet_command.rollerPara);
                break;

            }
            case E_FLEET_COMMAND_LIFT:
            {

                processResult=processLiftInstruction(tmp_fleet_command.liftPara);
                break;

            }
            case E_FLEET_COMMAND_CHARGE:
            {

                processResult=processChargeInstruction(tmp_fleet_command.chargePara);
                break;

            }
        case E_FLEET_COMMAND_LOAD_MAP:
        {

            processResult=processMapInstruction(tmp_fleet_command.mapPara);
            break;

        }
        case E_FLEET_COMMAND_WAIT:
        {
            processResult=processWaitInstruction();
            break;
        }
        case E_FLEET_COMMAND_WAITDI:
        {
            processResult=processWaitDiInstruction(tmp_fleet_command.waitDiPara);
            break;
        }
        case E_FLEET_COMMAND_SETDO:
        {
            processResult=processSetDoInstruction(tmp_fleet_command.setDoPara);
            break;
        }



            default:
            {
                qDebug()<<"unknow fleet command.";
                return -1;
            }

        }

        //updateTrajTime(), 偶尔出现，行号确实完成了,当是trajProfile还没有来得及清除，导致添加失败。
//        if(-1==processResult)
//        {
//            addMsg(ENUM_MSG_ERROR,"MotionServer","trajectory server",5021,robotId);
//            return -1;
//        }
        // 停止的时候跳出
//        sleep(0.005);
//        if(E_FLEET_TASK_STATUS_MOVING!=taskStatus)
//        {
//            motionDebugInfo.commonThreadStage=E_COMMON_THREAD_TRAJECTORY_CALCULATE_PATH_WHILE_BREAK;
//            return -1;
//        }
//    }
    motionDebugInfo.commonThreadStage=E_COMMON_THREAD_TRAJECTORY_CALCULATE_PATH_NORMAL_FINISH;
    return 1;
}




QString FleetService::longTimeToTimeString(long timeIn)
{
    time_t timep;
    struct tm* p;
    timep = timeIn;
    p=localtime(&timep);//从tm结构体中可以取到年月日时分秒等值。
    char szTmp[50] = {0};
    strftime(szTmp,50,"%Y-%m-%d %H:%M:%S",p);    //这里输出的将是 p的时间值

    return  QString(QLatin1String(szTmp));
}

//int FleetService::isMotionInstructionFinished(int waitLine)
//{
//    float lineStatus;
//    float floatLineDifference;
//    lineStatus=powerManage->getRunningNumber_virtualLineControl();
//    floatLineDifference=lineStatus-waitLine;
////    qDebug()<<"lineStatus="<<lineStatus<<",lineIndex="<<lineIndex;
//    if(floatLineDifference > 0.8 && floatLineDifference < 0.99)
//    {
//        return 1;
//    }

//    return -1;
//}

int FleetService::quitClear()
{
    powerManage->setUltrasonicSensorCheckEnable(true);
    powerManage->setPointCloudCheckEnable(true);
    powerManage->setCheckVehiclePosition(E_VEHICLE_POSITION_TYPE_ROS,true);
    return 1;
}

int FleetService::initialConfigFile_fleet(QString configFileName,QString &commentOut)
{
    qDebug() << "FleetService::initialConfigFile ==== 初始化配置文件：" << configFileName;

    DomParser domparser;
    bool ok = domparser.openXml(configFileName, QIODevice::ReadOnly);
    if( !ok )
    {
        qDebug() << "FleetService::initialConfigFile ==== 配置文件打开失败：" << configFileName;
        return -1;
    }

    QDomNode domNode = domparser.findSubNode(domparser.getRootDomElement(), "isUsePathAuthority", ok );
    if( !ok )
    {
        qDebug() << "FleetService::initialConfigFile failed" << configFileName;
        commentOut="isUsePathAuthority";
        return -1;
    }
    domparser.readXml(domNode, isUsePathAuthority);


    domparser.closeXml();
    return 1;
}



int FleetService::readConfigFile_mc(QString fileName)
{
    qDebug() << "MoveFreeInstructionLogic::readConfigFile ==== 初始化配置文件：" << fileName;

    DomParser domparser;
    bool ok = domparser.openXml(fileName, QIODevice::ReadOnly);
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "MoveFreeInstructionLogic::readConfigFile ==== 配置文件打开失败：" << fileName;
        }
        return -1;
    }

    QDomNode domNode_f = domparser.findSubNode(domparser.getRootDomElement(), "robotSetting", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "MoveFreeInstructionLogic::readConfigFile ==== 读取robotSetting结点失败：" << fileName;
        }
            return -2;
    }


    QDomNode domNode = domparser.findSubNode(domNode_f, "agv_lineMaxAcceleration", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "MoveFreeInstructionLogic::readConfigFile ==== 读取agv_lineMaxAcceleration结点失败：" << fileName;
        }
            return -2;
    }
    domparser.readXml(domNode, agv_lineMaxAcceleration);

    domNode = domparser.findSubNode(domNode_f, "agv_rotateMaxAcceleration", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "MoveFreeInstructionLogic::readConfigFile ==== 读取agv_rotateMaxAcceleration结点失败：" << fileName;
        }
            return -2;
    }
    domparser.readXml(domNode, agv_rotateMaxAcceleration);

    domNode_f = domparser.findSubNode(domparser.getRootDomElement(), "InstructionSetting", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "MoveFreeInstructionLogic::readConfigFile ==== 读取InstructionSetting结点失败：" << fileName;
        }
            return -2;
    }


    domNode = domparser.findSubNode(domNode_f, "vmark_lineAcceleration", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "MoveFreeInstructionLogic::readConfigFile ==== 读取vmark_lineAcceleration结点失败：" << fileName;
        }
            return -2;
    }
    domparser.readXml(domNode, vmark_lineAcceleration);

    domNode = domparser.findSubNode(domNode_f, "vmark_rotateAcceleration", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "MoveFreeInstructionLogic::readConfigFile ==== 读取vmark_rotateAcceleration结点失败：" << fileName;
        }
            return -2;
    }
    domparser.readXml(domNode, vmark_rotateAcceleration);

    domNode = domparser.findSubNode(domNode_f, "cu1PositionAllowAngleAccuracy", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "MoveFreeInstructionLogic::readConfigFile ==== 读取结点失败：" << fileName;
        }
            return -2;
    }
    domparser.readXml(domNode, cu1PositionAllowAngleAccuracy);


    domparser.closeXml();
    return 1;

}

#include <unistd.h>
#include <sys/syscall.h>
#define gettid() syscall(__NR_gettid)

void* FleetService::threadRun(void *)
{
    printf("FleetService theread start run !!!");
#if 0
    int max_cpus = sysconf(_SC_NPROCESSORS_ONLN);
    printf("Max CPUs = %d\n", max_cpus);

    /* lock all memory (prevent swapping) */
    if (mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
        printf("printf: mlockall\n");
    }

    set_latency_target();
    set_cpu(0);
#endif

    struct sched_param param;
    param.__sched_priority = 20;
    sched_setscheduler(0,SCHED_OTHER,&param);
    qDebug()<<"************************FleetService pid="<<gettid();
    usleep(789000);
    //setStackSize(81920);
    int policy, priority;
    pthread_getschedparam(pthread_self(), &policy, (struct sched_param *)&priority);
//    if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
//        qDebug("mlockall failed");
//    }
//    clock_gettime( CLOCK_MONOTONIC, &cycleTimespec );

   // qDebug()<<"this=="<<this;
   // pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL); //允许退出线程
    //pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL); //设置立即取消

    while(1)
    {
       // pthread_testcancel();/*the thread can be killed only here*/
        usleep(5000);
        taskLoop();
    }
    printf("FleetService thread quit!!!");
}

void* FleetService::threadTask( void* classPtr )
{
    qDebug()<<"FleetService thread run-------------------------";
    return ((FleetService*)classPtr)->threadRun(NULL);
}

void FleetService::createThread()
{
    int ret=pthread_create( &threadId, NULL, &FleetService::threadTask,this);
    qDebug()<<"FleetService createThread-------------------------";
}


void* FleetService::threadRun_traffic(void *)
{
    printf("FleetService theread_traffic start run !!!");
#if 0
    int max_cpus = sysconf(_SC_NPROCESSORS_ONLN);
    printf("Max CPUs = %d\n", max_cpus);

    /* lock all memory (prevent swapping) */
    if (mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
        printf("printf: mlockall\n");
    }

    set_latency_target();
    set_cpu(0);
#endif

    struct sched_param param;
    param.__sched_priority = 20;
    sched_setscheduler(0,SCHED_OTHER,&param);
    qDebug()<<"************************FleetService pid="<<gettid();
    usleep(789000);
    //setStackSize(81920);
    int policy, priority;
    pthread_getschedparam(pthread_self(), &policy, (struct sched_param *)&priority);
//    if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
//        qDebug("mlockall failed");
//    }
//    clock_gettime( CLOCK_MONOTONIC, &cycleTimespec );

   // qDebug()<<"this=="<<this;
   // pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL); //允许退出线程
    //pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL); //设置立即取消

    while(1)
    {
       // pthread_testcancel();/*the thread can be killed only here*/
        usleep(5000);
        trafficLoop();
    }
    printf("FleetService _traffic thread quit!!!");
}

void* FleetService::threadTask_traffic( void* classPtr )
{
    qDebug()<<"FleetService _traffic thread run-------------------------";
    return ((FleetService*)classPtr)->threadRun_traffic(NULL);
}

void FleetService::createThread_traffic()
{
    int ret=pthread_create( &threadId, NULL, &FleetService::threadTask_traffic,this);
    qDebug()<<"FleetService  _traffic createThread-------------------------";
}


void FleetService::addMsg(int messageLevel, string componentName, string messageType,
                           int messageCode, int robotId,
                           int parameter1, int parameter2, int parameter3, int parameter4
                          ,QString strIn)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;



    QString infomationStr;

    switch(messageCode)
    {
    case 12218:
    {
        infomationStr =  QObject::tr("agv下个站点(%1)未授权，进入暂停模式！")
                .arg(parameter1);
        break;
    }
    case 12217:
    {
        infomationStr =  QObject::tr("agv当前路径(%1)已授权，解除暂停模式！")
                .arg(parameter1);
        break;
    }
    case 12216:
    {
        infomationStr =  QObject::tr("agv当前路径(%1)未授权，进入暂停模式！")
                .arg(parameter1);
        break;
    }
    case 12215:
    {
        infomationStr =  QObject::tr("fleetService配置文件fleetServiceConfig.xml读取失败！%1")
                .arg(strIn);
        break;
    }
    case 12214:
    {
        infomationStr =  QObject::tr("路径规划失败:（当前未在站点上，且当前路径终点非绘图点、报告点、停靠点，请手动移动agv到任意绘图点附近）,开始站点%1,结束站点%2．所处路径id%3 ！")
                .arg(parameter1).arg(parameter2).arg(parameter3);
        break;
    }
    case 12213:
    {
        infomationStr =  QObject::tr("路径规划失败(路径不可达):（先运行完成当前所处路径）,开始站点%1,结束站点%2．所处路径id%3. ！")
                .arg(parameter1).arg(parameter2).arg(parameter3);
        break;
    }
    case 12212:
    {
        infomationStr =  QObject::tr("路径规划失败(路径不可达):(找到最近点进行),开始站点%1,结束站点%2．当前站点%3.过渡站点%4. ！")
                .arg(parameter1).arg(parameter2).arg(parameter3).arg(parameter4);
        break;
    }
    case 12211:
    {
        infomationStr =  QObject::tr("路径规划成功:sucess（先运行完成当前所处路径），开始站点%1,结束站点%2．所处路径id%3.！")
                .arg(parameter1).arg(parameter2).arg(parameter3);
        break;
    }
    case 12210:
    {
        infomationStr =  QObject::tr("路径规划成功:sucess（找到最近点进行），开始站点%1,结束站点%2．当前站点%3.过渡站点%4. ！")
                .arg(parameter1).arg(parameter2).arg(parameter3).arg(parameter4);
        break;
    }
    case 12209:
    {
        infomationStr =  QObject::tr("路径规划成功:sucess（正常开始点规划路径），开始站点%1,结束站点%2． ！")
                .arg(parameter1).arg(parameter2);
        break;
    }
    case 12208:
    {
        infomationStr =  QObject::tr("调度:非空闲状态，禁止执行关机命令！");
        break;
    }
    case 12207:
    {
        infomationStr =  QObject::tr("调度命令队列缓存有运动路径,无法响应自由导航指令！");
        break;
    }
    case 12206:
    {
        infomationStr =  QObject::tr("AGV当前状态非空闲状态,无法响应自由导航指令！");
        break;
    }
    case 12205:
    {
        infomationStr =  QObject::tr("多条平滑过渡曲线的中间，插入了禁止动作，请删除！")
                .arg(parameter1).arg(parameter2);
        break;
    }
    case 12204:
    {
        infomationStr =  QObject::tr("路径规划失败(路径不可达):(正常开始站点规划无路径),开始站点%1,结束站点%2． ！")
                .arg(parameter1).arg(parameter2);
        break;
    }
    case 12203:
    {
        infomationStr =  QObject::tr("调度命令（%1）解析失败,请检查AGV是否处于调度模式，而且设备注册码正常！")
                .arg(strIn);
        break;
    }
    case 12202:
    {
        infomationStr =  QObject::tr("当前AGV不处于调度模式，拒绝执行调度命令，请先切换为调度模式！")
                .arg(parameter1);
        break;
    }
    case 12201:
    {
        infomationStr =  QObject::tr("fleetService配置文件mcControllerInterpreter.xml读取失败！")
                .arg(parameter1);
        break;
    }
    case 12200:
    {
        infomationStr =  QObject::tr("超声波传感器（id号%1）通讯失败！")
                .arg(parameter1);
        break;
    }
    default:

        break;
    }


    tmpMsg.MessageInformation = infomationStr.toStdString();

    MessageLog::getInstance()->addMessage(tmpMsg);
}

